#ifndef CCALIBRATION_HPP
#define CCALIBRATION_HPP
#include <cstdio>
#include <ctime>
#include <iostream>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <sstream>
#include <string>

#include "common.h"

using namespace cv;
using namespace std;

class Settings {
 public:
  Settings() : goodInput(false) {}
  enum Pattern {
    NOT_EXISTING,
    CHESSBOARD,
    CIRCLES_GRID,
    ASYMMETRIC_CIRCLES_GRID
  };
  enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };

  void write(FileStorage &fs) const  // Write serialization for this class
  {
    fs << "{"
       << "BoardSize_Width" << boardSize.width << "BoardSize_Height"
       << boardSize.height << "Square_Size" << squareSize << "Calibrate_Pattern"
       << patternToUse << "Calibrate_NrOfFrameToUse" << nrFrames
       << "Calibrate_FixAspectRatio" << aspectRatio
       << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
       << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint

       << "Write_DetectedFeaturePoints" << writePoints
       << "Write_extrinsicParameters" << writeExtrinsics
       << "Write_paramFileName" << paramFileName

       << "Show_UndistortedImage" << showUndistorsed

       << "Input_FlipAroundHorizontalAxis" << flipVertical << "Input_Delay"
       << delay << "Input" << input << "}";
  }
  void read(const FileNode &node)  // Read serialization for this class
  {
    node["BoardSize_Width"] >> boardSize.width;
    node["BoardSize_Height"] >> boardSize.height;
    node["Calibrate_Pattern"] >> patternToUse;
    node["Square_Size"] >> squareSize;
    node["Calibrate_NrOfFrameToUse"] >> nrFrames;
    node["Calibrate_FixAspectRatio"] >> aspectRatio;
    node["Write_DetectedFeaturePoints"] >> writePoints;
    node["Write_extrinsicParameters"] >> writeExtrinsics;
    node["Write_outputFileName"] >> paramFileName;
    node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
    node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
    node["Calibrate_UseFisheyeModel"] >> useFisheye;
    node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
    node["Show_UndistortedImage"] >> showUndistorsed;
    node["Input"] >> input;
    node["Input_Delay"] >> delay;
    node["Fix_K1"] >> fixK1;
    node["Fix_K2"] >> fixK2;
    node["Fix_K3"] >> fixK3;
    node["Fix_K4"] >> fixK4;
    node["Fix_K5"] >> fixK5;

    validate();
  }
  void validate() {
    goodInput = true;
    if (boardSize.width <= 0 || boardSize.height <= 0) {
      cerr << "Invalid Board size: " << boardSize.width << " "
           << boardSize.height << endl;
      goodInput = false;
    }
    if (squareSize <= 10e-6) {
      cerr << "Invalid square size " << squareSize << endl;
      goodInput = false;
    }
    if (nrFrames <= 0) {
      cerr << "Invalid number of frames " << nrFrames << endl;
      goodInput = false;
    }

    if (input.empty())  // Check for valid input
      inputType = INVALID;
    else {
      if (input[0] >= '0' && input[0] <= '9') {
        stringstream ss(input);
        ss >> cameraID;
        inputType = CAMERA;
      } else {
        if (isListOfImages(input) && readStringList(input, imageList)) {
          inputType = IMAGE_LIST;
          nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames
                                                        : (int)imageList.size();
        } else
          inputType = VIDEO_FILE;
      }
      // if (inputType == CAMERA)
      //      inputCapture.open(cameraID);
      // if (inputType == VIDEO_FILE)
      //     inputCapture.open(input);
      if (inputType != IMAGE_LIST && !inputCapture.isOpened())
        inputType = INVALID;
    }

    flag = 0;
    if (calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
    if (calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST;
    if (aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO;
    if (fixK1) flag |= CALIB_FIX_K1;
    if (fixK2) flag |= CALIB_FIX_K2;
    if (fixK3) flag |= CALIB_FIX_K3;
    if (fixK4) flag |= CALIB_FIX_K4;
    if (fixK5) flag |= CALIB_FIX_K5;

    if (useFisheye) {
      // the fisheye model has its own enum, so overwrite the flags
      flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC;
      if (fixK1) flag |= fisheye::CALIB_FIX_K1;
      if (fixK2) flag |= fisheye::CALIB_FIX_K2;
      if (fixK3) flag |= fisheye::CALIB_FIX_K3;
      if (fixK4) flag |= fisheye::CALIB_FIX_K4;
      if (calibFixPrincipalPoint) flag |= fisheye::CALIB_FIX_PRINCIPAL_POINT;
    }

    calibrationPattern = NOT_EXISTING;
    if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
    if (!patternToUse.compare("CIRCLES_GRID"))
      calibrationPattern = CIRCLES_GRID;
    if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID"))
      calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
    if (calibrationPattern == NOT_EXISTING) {
      cerr << " Camera calibration mode does not exist: " << patternToUse
           << endl;
      goodInput = false;
    }
    atImageList = 0;
  }
  Mat nextImage() {
    Mat result;
    if (inputCapture.isOpened()) {
      Mat view0;
      inputCapture >> view0;
      view0.copyTo(result);
    } else if (atImageList < imageList.size())
      result = imread(imageList[atImageList++], IMREAD_COLOR);

    return result;
  }

  static bool readStringList(const string &filename, vector<string> &l) {
    l.clear();
    FileStorage fs(filename, FileStorage::READ);
    if (!fs.isOpened()) return false;
    FileNode n = fs.getFirstTopLevelNode();
    if (n.type() != FileNode::SEQ) return false;
    FileNodeIterator it = n.begin(), it_end = n.end();
    for (; it != it_end; ++it) l.push_back((string)*it);
    return true;
  }

  static bool isListOfImages(const string &filename) {
    string setting(filename);
    // Look for file extension
    if (setting.find(".xml") == string::npos &&
        setting.find(".yaml") == string::npos &&
        setting.find(".yml") == string::npos)
      return false;
    else
      return true;
  }

 public:
  Size boardSize;  // The size of the board -> Number of items by width and
                   // height
  Pattern calibrationPattern;  // One of the Chessboard, circles, or asymmetric
                               // circle pattern
  float squareSize;  // The size of a square in your defined unit (point,
                     // millimeter,etc).
  int nrFrames;  // The number of frames to use from the input for calibration
  float aspectRatio;            // The aspect ratio
  int delay;                    // In case of a video input
  bool writePoints;             // Write detected feature points
  bool writeExtrinsics;         // Write extrinsic parameters
  bool calibZeroTangentDist;    // Assume zero tangential distortion
  bool calibFixPrincipalPoint;  // Fix the principal point at the center
  bool flipVertical;     // Flip the captured images around the horizontal axis
  string paramFileName;  // The name of the file where to write
  bool showUndistorsed;  // Show undistorted images after calibration
  string input;          // The input ->
  bool useFisheye;       // use fisheye camera model for calibration
  bool fixK1;            // fix K1 distortion coefficient
  bool fixK2;            // fix K2 distortion coefficient
  bool fixK3;            // fix K3 distortion coefficient
  bool fixK4;            // fix K4 distortion coefficient
  bool fixK5;            // fix K5 distortion coefficient

  int cameraID;
  vector<string> imageList;
  size_t atImageList;
  VideoCapture inputCapture;
  InputType inputType;
  bool goodInput;
  int flag;

 private:
  string patternToUse;
};

static inline void read(const FileNode &node, Settings &x,
                        const Settings &default_value = Settings()) {
  if (node.empty())
    x = default_value;
  else
    x.read(node);
}

static inline void write(FileStorage &fs, const String &,
                         const Settings &setting) {
  setting.write(fs);
}

enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2, CLEAR = 3 };

class CCalibration {
 private:
  /* data */
  Size mImageSize;
  vector<vector<Point2f>> mImageCornerPoints;
  clock_t prevTimestamp;

 public:
  CCalibration(/* args */);
  ~CCalibration();
  Settings setting;
  void Run(Mat &view);
  void InitDistCoeffs(void);
  int mMode;
  Mat mCameraMatrix, mDistCoeffs;
  bool mShowCornerPoints;
};

CCalibration::CCalibration(/* args */) {
  const string inputSettingsFile = "config.xml";
  FileStorage fs(inputSettingsFile, FileStorage::READ);  // Read the settings
  if (!fs.isOpened()) {
    // cout << "Could not open the configuration file: \"" << inputSettingsFile
    //      << "\"" << endl;
    // return -1;
  }
  fs["Settings"] >> setting;
  fs.release();  // close Settings file
  mMode = CAPTURING;
  mShowCornerPoints = false;
  prevTimestamp = 0;
}

CCalibration::~CCalibration() {}

void CCalibration::InitDistCoeffs() {
  mDistCoeffs = Mat::zeros(5, 1, CV_64F);
  mCameraMatrix = Mat::eye(3, 3, CV_64F);
  const string camParasFile = "camera_parameters.xml";
  FileStorage fs(camParasFile, FileStorage::READ);  // Read the settings
  if (!fs.isOpened()) {
    cout << "Could not open the camera_parameters file: \"" << camParasFile
         << "\"" << endl;
    // return -1;
  } else {
    cout << "Successed to open the camera_parameters file: \"" << camParasFile
         << "\"" << endl;
    fs["distortion_coefficients"] >> mDistCoeffs;
    fs["camera_matrix"] >> mCameraMatrix;
    cout << mDistCoeffs << endl;
    cout << mCameraMatrix << endl;
    gDistCoeffsInited = true;
  }
  fs.release();  // close Settings file
}

bool runCalibrationAndSave(Settings &setting, Size ImageSize, Mat &CameraMatrix,
                           Mat &DistCoeffs,
                           vector<vector<Point2f>> ImageCornerPoints);

void CCalibration::Run(Mat &view) {
  if (mMode == CLEAR) {
    mImageCornerPoints.clear();
    mMode = CAPTURING;
  }
  cout << "mImageCornerPoints.size(): " << mImageCornerPoints.size() << endl;
  //! [get_input]
  bool blinkOutput = false;
  //-----  If no more image, or got enough, then stop calibration and show
  // result -------------
  if (mMode == CAPTURING &&
      mImageCornerPoints.size() >= (size_t)setting.nrFrames) {
    if (runCalibrationAndSave(setting, mImageSize, mCameraMatrix, mDistCoeffs,
                              mImageCornerPoints))
      mMode = CALIBRATED;
    // else
    //     mMode = DETECTION;
  }
  if (view.empty())  // If there are no more images stop the loop
  {
    // if calibration threshold was not reached yet, calibrate now
    if (mMode != CALIBRATED && !mImageCornerPoints.empty())
      runCalibrationAndSave(setting, mImageSize, mCameraMatrix, mDistCoeffs,
                            mImageCornerPoints);
    mMode = CALIBRATED;
  }
  //! [get_input]

  mImageSize = view.size();  // Format input image.

  //! [find_pattern]
  vector<Point2f> pointBuf;
  bool pattern_found;
  int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
  switch (
      setting.calibrationPattern)  // Find feature points on the input format
  {
    case Settings::CHESSBOARD:
      pattern_found = findChessboardCorners(view, setting.boardSize, pointBuf,
                                            chessBoardFlags);
      break;
    case Settings::CIRCLES_GRID:
      pattern_found = findCirclesGrid(view, setting.boardSize, pointBuf);
      break;
    case Settings::ASYMMETRIC_CIRCLES_GRID:
      pattern_found = findCirclesGrid(view, setting.boardSize, pointBuf,
                                      CALIB_CB_ASYMMETRIC_GRID);
      break;
    default:
      pattern_found = false;
      break;
  }
  //! [find_pattern]

  if (pattern_found) {
    // improve the pattern_found corners' coordinate accuracy for chessboard
    if (setting.calibrationPattern == Settings::CHESSBOARD) {
      Mat viewGray;
      if (view.channels() == 3)
        cvtColor(view, viewGray, COLOR_BGR2GRAY);
      else
        viewGray = view;
      cornerSubPix(
          viewGray, pointBuf, Size(11, 11), Size(-1, -1),
          TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));
    }

    if (mMode ==
            CAPTURING &&  // For camera only take new samples after delay time
        (clock() - prevTimestamp > setting.delay * 1e-3 * CLOCKS_PER_SEC)) {
      mImageCornerPoints.push_back(pointBuf);
      prevTimestamp = clock();
      // blinkOutput = setting.inputCapture.isOpened();
    }

    // Draw the corners.
    if (mShowCornerPoints)
      drawChessboardCorners(view, setting.boardSize, Mat(pointBuf),
                            pattern_found);
  }
  //! [pattern_found]

  //----------------------------- Output Text
  //------------------------------------------------
  //! [output_text]
  string msg = (mMode == CAPTURING)
                   ? "100/100"
                   : mMode == CALIBRATED ? "Calibrated" : "Detection";
  int baseLine = 0;
  Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
  Point textOrigin(view.cols - 2 * textSize.width - 10,
                   view.rows - 2 * baseLine - 10);

  if (mMode == CAPTURING) {
    if (setting.showUndistorsed)
      msg = format("%d/%d Undist", (int)mImageCornerPoints.size(),
                   setting.nrFrames);
    else
      msg = format("%d/%d", (int)mImageCornerPoints.size(), setting.nrFrames);
  }

  putText(view, msg, textOrigin, 1, 1, mMode == CALIBRATED ? GREEN : RED);

  if (blinkOutput) bitwise_not(view, view);
  //! [output_text]
  //------------------------- Video capture  output  undistorted
  //------------------------------
  //! [output_undistorted]
  if (mMode == CALIBRATED && setting.showUndistorsed) {
    Mat temp = view.clone();
    if (setting.useFisheye)
      cv::fisheye::undistortImage(temp, view, mCameraMatrix, mDistCoeffs);
    else
      undistort(temp, view, mCameraMatrix, mDistCoeffs);
  }
}

//! [compute_errors]
static double computeReprojectionErrors(
    const vector<vector<Point3f>> &objectPoints,
    const vector<vector<Point2f>> &ImageCornerPoints, const vector<Mat> &rvecs,
    const vector<Mat> &tvecs, const Mat &CameraMatrix, const Mat &DistCoeffs,
    vector<float> &perViewErrors, bool fisheye) {
  vector<Point2f> imagePoints2;
  size_t totalPoints = 0;
  double totalErr = 0, err;
  perViewErrors.resize(objectPoints.size());

  for (size_t i = 0; i < objectPoints.size(); ++i) {
    if (fisheye) {
      fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i],
                             CameraMatrix, DistCoeffs);
    } else {
      projectPoints(objectPoints[i], rvecs[i], tvecs[i], CameraMatrix,
                    DistCoeffs, imagePoints2);
    }
    err = norm(ImageCornerPoints[i], imagePoints2, NORM_L2);

    size_t n = objectPoints[i].size();
    perViewErrors[i] = (float)std::sqrt(err * err / n);
    totalErr += err * err;
    totalPoints += n;
  }

  return std::sqrt(totalErr / totalPoints);
}
//! [compute_errors]
//! [board_corners]
static void calcBoardCornerPositions(
    Size boardSize, float squareSize, vector<Point3f> &corners,
    Settings::Pattern patternType /*= Settings::CHESSBOARD*/) {
  corners.clear();

  switch (patternType) {
    case Settings::CHESSBOARD:
    case Settings::CIRCLES_GRID:
      for (int i = 0; i < boardSize.height; ++i)
        for (int j = 0; j < boardSize.width; ++j)
          corners.push_back(Point3f(j * squareSize, i * squareSize, 0));
      break;

    case Settings::ASYMMETRIC_CIRCLES_GRID:
      for (int i = 0; i < boardSize.height; i++)
        for (int j = 0; j < boardSize.width; j++)
          corners.push_back(
              Point3f((2 * j + i % 2) * squareSize, i * squareSize, 0));
      break;
    default:
      break;
  }
}
//! [board_corners]
static bool runCalibration(Settings &setting, Size &ImageSize,
                           Mat &CameraMatrix, Mat &DistCoeffs,
                           vector<vector<Point2f>> ImageCornerPoints,
                           vector<Mat> &rvecs, vector<Mat> &tvecs,
                           vector<float> &reprojErrs, double &totalAvgErr) {
  //! [fixed_aspect]
  CameraMatrix = Mat::eye(3, 3, CV_64F);
  if (setting.flag & CALIB_FIX_ASPECT_RATIO)
    CameraMatrix.at<double>(0, 0) = setting.aspectRatio;
  //! [fixed_aspect]
  if (setting.useFisheye) {
    DistCoeffs = Mat::zeros(4, 1, CV_64F);
  } else {
    DistCoeffs = Mat::zeros(8, 1, CV_64F);
  }

  vector<vector<Point3f>> objectPoints(1);
  calcBoardCornerPositions(setting.boardSize, setting.squareSize,
                           objectPoints[0], setting.calibrationPattern);

  objectPoints.resize(ImageCornerPoints.size(), objectPoints[0]);

  // Find intrinsic and extrinsic camera parameters
  double rms;
  rms = calibrateCamera(objectPoints, ImageCornerPoints, ImageSize,
                        CameraMatrix, DistCoeffs, rvecs, tvecs, setting.flag);

  cout << "Re-projection error reported by calibrateCamera: " << rms << endl;

  bool ok = checkRange(CameraMatrix) && checkRange(DistCoeffs);

  totalAvgErr = computeReprojectionErrors(
      objectPoints, ImageCornerPoints, rvecs, tvecs, CameraMatrix, DistCoeffs,
      reprojErrs, setting.useFisheye);

  return ok;
}

// Write camera parameters to the output file
static void saveCameraParams(Settings &setting, Size &ImageSize,
                             Mat &CameraMatrix, Mat &DistCoeffs,
                             const vector<Mat> &rvecs, const vector<Mat> &tvecs,
                             const vector<float> &reprojErrs,
                             const vector<vector<Point2f>> &ImageCornerPoints,
                             double totalAvgErr) {
  FileStorage fs(setting.paramFileName, FileStorage::WRITE);

  time_t tm;
  time(&tm);
  struct tm *t2 = localtime(&tm);
  char buf[1024];
  strftime(buf, sizeof(buf), "%c", t2);

  fs << "calibration_time" << buf;

  if (!rvecs.empty() || !reprojErrs.empty())
    fs << "nr_of_frames" << (int)std::max(rvecs.size(), reprojErrs.size());
  fs << "image_width" << ImageSize.width;
  fs << "image_height" << ImageSize.height;
  fs << "board_width" << setting.boardSize.width;
  fs << "board_height" << setting.boardSize.height;
  fs << "square_size" << setting.squareSize;

  if (setting.flag & CALIB_FIX_ASPECT_RATIO)
    fs << "fix_aspect_ratio" << setting.aspectRatio;

  if (setting.flag) {
    std::stringstream flagsStringStream;
    if (setting.useFisheye) {
      flagsStringStream
          << "flags:"
          << (setting.flag & fisheye::CALIB_FIX_SKEW ? " +fix_skew" : "")
          << (setting.flag & fisheye::CALIB_FIX_K1 ? " +fix_k1" : "")
          << (setting.flag & fisheye::CALIB_FIX_K2 ? " +fix_k2" : "")
          << (setting.flag & fisheye::CALIB_FIX_K3 ? " +fix_k3" : "")
          << (setting.flag & fisheye::CALIB_FIX_K4 ? " +fix_k4" : "")
          << (setting.flag & fisheye::CALIB_RECOMPUTE_EXTRINSIC
                  ? " +recompute_extrinsic"
                  : "");
    } else {
      flagsStringStream
          << "flags:"
          << (setting.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess"
                                                       : "")
          << (setting.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "")
          << (setting.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point"
                                                       : "")
          << (setting.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist"
                                                     : "")
          << (setting.flag & CALIB_FIX_K1 ? " +fix_k1" : "")
          << (setting.flag & CALIB_FIX_K2 ? " +fix_k2" : "")
          << (setting.flag & CALIB_FIX_K3 ? " +fix_k3" : "")
          << (setting.flag & CALIB_FIX_K4 ? " +fix_k4" : "")
          << (setting.flag & CALIB_FIX_K5 ? " +fix_k5" : "");
    }
    fs.writeComment(flagsStringStream.str());
  }

  fs << "flags" << setting.flag;

  fs << "fisheye_model" << setting.useFisheye;

  fs << "camera_matrix" << CameraMatrix;
  fs << "distortion_coefficients" << DistCoeffs;

  fs << "avg_reprojection_error" << totalAvgErr;
  if (setting.writeExtrinsics && !reprojErrs.empty())
    fs << "per_view_reprojection_errors" << Mat(reprojErrs);

  if (setting.writeExtrinsics && !rvecs.empty() && !tvecs.empty()) {
    CV_Assert(rvecs[0].type() == tvecs[0].type());
    Mat bigmat((int)rvecs.size(), 6, CV_MAKETYPE(rvecs[0].type(), 1));
    bool needReshapeR = rvecs[0].depth() != 1 ? true : false;
    bool needReshapeT = tvecs[0].depth() != 1 ? true : false;

    for (size_t i = 0; i < rvecs.size(); i++) {
      Mat r = bigmat(Range(int(i), int(i + 1)), Range(0, 3));
      Mat t = bigmat(Range(int(i), int(i + 1)), Range(3, 6));

      if (needReshapeR)
        rvecs[i].reshape(1, 1).copyTo(r);
      else {
        //*.t() is MatExpr (not Mat) so we can use assignment operator
        CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
        r = rvecs[i].t();
      }

      if (needReshapeT)
        tvecs[i].reshape(1, 1).copyTo(t);
      else {
        CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
        t = tvecs[i].t();
      }
    }
    fs.writeComment(
        "a set of 6-tuples (rotation vector + translation vector) "
        "for each view");
    fs << "extrinsic_parameters" << bigmat;
  }

  if (setting.writePoints && !ImageCornerPoints.empty()) {
    Mat imagePtMat((int)ImageCornerPoints.size(),
                   (int)ImageCornerPoints[0].size(), CV_32FC2);
    for (size_t i = 0; i < ImageCornerPoints.size(); i++) {
      Mat r = imagePtMat.row(int(i)).reshape(2, imagePtMat.cols);
      Mat imgpti(ImageCornerPoints[i]);
      imgpti.copyTo(r);
    }
    fs << "image_points" << imagePtMat;
  }
}

//! [run_and_save]
bool runCalibrationAndSave(Settings &setting, Size ImageSize, Mat &CameraMatrix,
                           Mat &DistCoeffs,
                           vector<vector<Point2f>> ImageCornerPoints) {
  vector<Mat> rvecs, tvecs;
  vector<float> reprojErrs;
  double totalAvgErr = 0;

  bool ok =
      runCalibration(setting, ImageSize, CameraMatrix, DistCoeffs,
                     ImageCornerPoints, rvecs, tvecs, reprojErrs, totalAvgErr);
  cout << (ok ? "Calibration succeeded" : "Calibration failed")
       << ". avg re projection error = " << totalAvgErr << endl;

  if (ok)
    saveCameraParams(setting, ImageSize, CameraMatrix, DistCoeffs, rvecs, tvecs,
                     reprojErrs, ImageCornerPoints, totalAvgErr);
  return ok;
}
//! [run_and_save]
#endif
